#include <iostream>
#include <stdio.h>
#include "opencv2/core.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/core/ocl.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/xfeatures2d.hpp"
#include "opencv2/ml.hpp"
#include "image_stitch.hpp"
#include "sort_algo.hpp"

using namespace cv;
using namespace std;
using namespace cv::xfeatures2d;
using namespace cv::ml;

const int maxn = 100;

int left_index[maxn] = {-1};
int right_index[maxn] = {-1};
int vis[maxn];
Mat img[maxn];

int n = 6;

class ImgMatch
{
private:
public:
    int il, ir;
    float dist;
    ImgMatch(int il, int ir, float dist)
    {
        this->il = il;
        this->ir = ir;
        this->dist = dist;
    }
    bool operator<(ImgMatch &rhs)
    {
        return dist < rhs.dist;
    }
    friend ostream &operator<<(ostream &os, ImgMatch &rhs)
    {
        os << rhs.il << " " << rhs.ir << " " << rhs.dist << endl;
        return os;
    }
};

vector<ImgMatch> img_matches;

void image_stitch_iter(int ir, Mat &res)
{
    float quality;
    IMG_ORDER order;
    vis[ir] = 1;
    try_match(res, img[ir], res, quality, order);
    if (right_index[ir] != -1)
    {
        image_stitch_iter(right_index[ir], res);
    }
}

void sort_algo(vector<string> &filenames, vector<Mat> &results, vector<Mat> &failures)
{
    memset(left_index, -1, sizeof(left_index));
    memset(right_index, -1, sizeof(right_index));

    n = filenames.size();

    for (int i = 0; i < n; i++)
    {
        img[i] = imread(filenames[i], 1);
        resize(img[i], img[i], Size(1024, 720), 0, 0, INTER_AREA);
    }
    img_matches.clear();
    for (int i = 0; i < n; i++)
    {

        for (int j = i + 1; j < n; j++)
        {
            bool matched;
            float quality;
            Mat res;
            IMG_ORDER order;

            matched = try_match(img[i], img[j], res, quality, order);
            if (matched)
            {
                if (order == ORDER_RIGHT)
                {
                    img_matches.push_back(ImgMatch(j, i, quality));
                }
                else
                {
                    img_matches.push_back(ImgMatch(i, j, quality));
                }

                // imshow("opm_surf_result", res);
                // imwrite("opm_surf_result.jpg", res);
                // waitKey();
            }
        }
    }
    sort(img_matches.begin(), img_matches.end());

    for (int i = 0; i < img_matches.size(); i++)
    {
        // cout << img_matches[i]<< endl;
        int il, ir;
        il = img_matches[i].il;
        ir = img_matches[i].ir;
        if (left_index[ir] == -1)
        {
            // cout << il << " " << ir <<  endl;
            left_index[ir] = il;
            right_index[il] = ir;
        }
    }

    Mat res;
    int cnt = 0;
    for (int i = 0; i < n; i++)
    {
        // cout << i << " " << left_index[i] << " " << right_index[i] <<  endl;
        if (right_index[i] != -1 && left_index[i] == -1 && vis[i] != 1)
        {
            // cout << i << "i\n";
            res = img[i];
            image_stitch_iter(right_index[i], res);
            results.push_back(res);
            // imshow("orb_stitch_res" + to_string(cnt), res);
            // imwrite("orb_stitch_res" + to_string(cnt++) + ".jpg", res);
            // waitKey(1000);
        }
    }

    for (int i = 0; i < n; i++)
    {
        if ((left_index[i] == -1 && right_index[i] == -1))
        {
            //cout << i << endl;
            failures.push_back(img[i]);

            // imshow("cannot register", img[i]);
            // waitKey(1000);
        }
    }
    // waitKey(0);
}